import rclpy
from rclpy.node import Node #Node 类
import rclpy.time
from tf2_ros import TransformListener,Buffer
from tf_transformations import euler_from_quaternion #四元数转换为欧拉角
import math,time


class TFlistener(Node):
    def __init__(self,node_name):
        super().__init__(node_name)
        self.buffer_ = Buffer()
        self.listener_= TransformListener(self.buffer_,self)
        self.timer_ = self.create_timer(1.0,self.get_transform)

    def get_transform(self):
        '''
        定时获取坐标关系

        '''
        try:
            result = self.buffer_.lookup_transform('map','base_footprint',rclpy.time.Time(seconds=0.0),rclpy.time.Duration(seconds = 1.0))
            transform = result.transform
            self.get_logger().info(f'平移:{transform.translation}')
            self.get_logger().info(f'旋转:{transform.rotation}')
            rotation_euler = euler_from_quaternion(
                [
                transform.rotation.x,
                transform.rotation.y,
                transform.rotation.z,
                transform.rotation.w,
                ]
            )
            self.get_logger().info(f'旋转RPY:{rotation_euler}')
        except Exception as e:
            self.get_logger().warn(f'获取坐标变换失败:原因{str(e)}')
            

def main():
    rclpy.init()
    node = TFlistener("listener_TF")
    rclpy.spin(node)
    rclpy.shutdown()

if __name__=='__main__':
    main()